
#include <vector>
using std::vector;

#include <list>
using std::list;

#include <string>
using std::string;

#include <iostream>
using std::cout;
using std::endl;
using std::cin;

#include <cmath>
#include <cstdlib>

//#include <windows.h>
#include <fstream>
using std::ifstream;
using std::ofstream;

/*
Add vertices on each edge pixel except those on straight edges.
Remove vertices for which the gradients (or angles) in front and behind differ by some absolute tolerance (floating-point tolerance + anti-aliasing tolerance, not a desired tolerance).

Run the algo starting at each pixel, but quickly ignore cases where the sample is completely black or completely white

Using a multi-valued density function (i.e., greyscale image) would fix the
aliasing problem.
Solution: Use an anti-aliased image!
*/

//#define DEBUG

#ifdef DEBUG
#define DBG_PRINT(x) std::cout << x;
#else
#define DBG_PRINT(x)
#endif

/* Nice and simple image object */
struct Image {
    enum Colour {
        COLOUR_RED = 0,
        COLOUR_GREEN,
        COLOUR_BLUE,
        COLOUR_ALPHA
    };
    vector<unsigned char> data;
    int width;
    int height;
    unsigned char bytes_per_pixel;
};

enum ImageLoadRetVal {
    IMAGE_SUCCESS = 0,
    IMAGE_FAILED_TO_OPEN_FILE,
    IMAGE_INVALID_TYPE,
    IMAGE_INVALID_BIT_DEPTH,
    IMAGE_WRITE_FAILED
};

/* TODO: Replace these loading and saving functions with the
         updated versions from uni, as these are very dodgy.
*/
int loadTGAImage(const char* filename, Image* image)
{
	FILE *file;
	unsigned char type[4];
	unsigned char info[6];
    
    file = fopen(filename, "rb");
    
    if (!file)
        return IMAGE_FAILED_TO_OPEN_FILE;
    
	fread (&type, sizeof (char), 3, file);
	fseek (file, 12, SEEK_SET);
	fread (&info, sizeof (char), 6, file);
    
	// Can only load images of type 2 (color) or 3 (greyscale)
	// TODO: Can this actually load greyscale TGAs?
	if (type[1] != 0 || (type[2] != 2 && type[2] != 3))
	{
		fclose(file);
		return IMAGE_INVALID_TYPE;
	}
    
	image->width = info[0] + info[1] * 256;
	image->height = info[2] + info[3] * 256;
	image->bytes_per_pixel = info[4] / 8;
    
	if (image->bytes_per_pixel != 3 && image->bytes_per_pixel != 4) {
		fclose(file);
		return IMAGE_INVALID_BIT_DEPTH;
	}
    
	long image_size = image->width * image->height * image->bytes_per_pixel;
    
	// Allocate memory for the image data
	image->data.resize(image_size, 0);
    
	// Read in the image data
	fread(&image->data[0], sizeof(unsigned char), image_size, file);
    
    // Change BGR to RGB (Targa is weird)
    for( int i=0; i<image_size; i+=image->bytes_per_pixel ) {
        // Swap blue and red colour values
        unsigned char red = image->data[i];
        image->data[i] = image->data[i+2];
        image->data[i+2] = red;
        //image->data[i] ^= image->data[i+2] ^=
        //        image->data[i] ^= image->data[i+2];
    }
    
	// Close the file
	fclose(file);
    
	return IMAGE_SUCCESS;
}

int saveTGAImage(Image* image, const char* filename)
{
    FILE *file;
	unsigned char type[4];
	unsigned char info[6];
	
	typedef unsigned short WORD;
	
    file = fopen(filename, "wb");
    
    if (!file)
        return IMAGE_FAILED_TO_OPEN_FILE;
    
    int bytes_written;
    
    type[0] = 0;
    type[1] = 0; // TODO: What's this for?
    type[2] = 2; // Colour image
	bytes_written = fwrite (&type, sizeof (char), 3, file);
	if( bytes_written != 3 )
	    return IMAGE_WRITE_FAILED;
	
	fseek (file, 12, SEEK_SET);
	
	//(*(WORD*)&info[0]) = (WORD)image->width;
	//(*(WORD*)&info[2]) = (WORD)image->height;
	//info[0] = image->width % 256;
	//info[1] = image->width / 256;
	//info[2] = image->height % 256;
	//info[3] = image->height / 256;
	info[0] = image->width & 0x00FF;
	info[1] = (image->width & 0xFF00) / 256;
	info[2] = image->height & 0x00FF;
	info[3] = (image->height & 0xFF00) / 256;
	info[4] = image->bytes_per_pixel * 8;
	info[5] = 0;
	
	bytes_written = fwrite (&info, sizeof (char), 6, file);
	if( bytes_written != 6 )
	    return IMAGE_WRITE_FAILED;
	
	long image_size = image->width * image->height * image->bytes_per_pixel;
    
    // Change RGB to BGR (Targa is weird)
    for( int i=0; i<image_size; i+=image->bytes_per_pixel ) {
        // Swap blue and red colour values
        unsigned char red = image->data[i];
        image->data[i] = image->data[i+2];
        image->data[i+2] = red;
        //image->data[i] ^= image->data[i+2] ^=
        //        image->data[i] ^= image->data[i+2];
    }
    
	// Write the image data
	bytes_written = fwrite(&image->data[0], sizeof(unsigned char), image_size, file);
    if( bytes_written != image_size )
	    return IMAGE_WRITE_FAILED;
    
    // Close the file
	fclose(file);
    
    // No-one will ever know (i.e., change it back again)
    for( int i=0; i<image_size; i+=image->bytes_per_pixel ) {
        // Swap blue and red colour values
        unsigned char red = image->data[i];
        image->data[i] = image->data[i+2];
        image->data[i+2] = red;
        //image->data[i] ^= image->data[i+2] ^=
        //        image->data[i] ^= image->data[i+2];
    }
    
	return IMAGE_SUCCESS;
}

// TODO: Unify this with the getPixel function below!
double sampleBilinearRepeat(const Image& image, double u, double v, uchar channel=0)
{
    int w = image.width;
    int h = image.height;
    u *= w;
    v *= h;
    int x = (int)floor(u);
    int y = (int)floor(v);
    int xn = x+1;
    int yn = y+1;
    double p = u - x;
    double q = v - y;
    double np = 1-p;
    double nq = 1-q;
    x %= w; xn %= w;
    y %= h; yn %= h;
    unsigned char pxl[4] = {
        image.data[(x+y*w)*image.bytes_per_pixel+channel],
        image.data[(xn+y*w)*image.bytes_per_pixel+channel],
        image.data[(x+yn*w)*image.bytes_per_pixel+channel],
        image.data[(xn+yn*w)*image.bytes_per_pixel+channel]
    };
    return (pxl[0]*np + pxl[1]*p) * nq +
           (pxl[2]*np + pxl[3]*p) * q;
}

int genTerrain(Image& out_image, int width, int height,
               unsigned int seed=0,
               double density_bias=0.0,
               int octave_count=9, 
               double octave_freqs[]=0,
               double octave_ampls[]=0,
               int warp_count=1,
               double warp_freqs[]=0,
               double warp_amples[]=0)
{
    // Build a bunch of random-noise lookup-tables (Images)
    // -------------------------------------------
    const int noise_channels = 9;
    const int noise_x_samples = 32;
    const int noise_y_samples = 32;
    const int noise_samples = noise_x_samples*
                              noise_y_samples;
    // Seed the random number generator
    srand(seed);
    Image noise[noise_channels];
    for( int channel=0; channel!=noise_channels; ++channel ) {
        noise[channel].width = noise_x_samples;
        noise[channel].height = noise_y_samples;
        noise[channel].bytes_per_pixel = 1;
        noise[channel].data.resize(noise_samples);
        for( int i=0; i!=noise_samples; ++i )
            // Crude but easy random sample
            noise[channel].data[i] = rand() % 256;// - 128;
    }
    // -------------------------------------------
    
    // Generate the terrain
    double density[width*height];
    double max_density = -1e99;
    double min_density = 1e99;
    for( int y=0; y!=height; ++y ) {
        for( int x=0; x!=width; ++x ) {
            // Get the u, v coordinates
            double u = (double)x / (double)width;
            double v = (double)y / (double)height;
            
            //cout << "u, v = " << u << ", " << v << endl;
            
            //cout << "sample = " << 
            //        sampleBilinearRepeat(noise[0], u, v) / 128 - 1.0 <<
            //        endl;
            
            // Shortcut
            double& d = density[x+y*width];
            
            // Set initial density
            d = density_bias;
            
            // Apply warping of the coordinates
            for( int i=0; i!=warp_count; ++i ) {
                u += (sampleBilinearRepeat(
                          noise[i%noise_channels],
                          u * warp_freqs[i]/noise_x_samples,
                          v * warp_freqs[i]/noise_y_samples
                       ) * warp_amples[i]) / 128.0 - 1.0;
                u += (sampleBilinearRepeat(
                          noise[(i+1)%noise_channels],
                          u * warp_freqs[i],
                          v * warp_freqs[i]
                       ) * warp_amples[i]) / 128.0 - 1.0;
            }
            
            // Add octaves of noise
            for( int i=0; i!=octave_count; ++i ) {
                d += (sampleBilinearRepeat(
                         noise[i%noise_channels],
                         u * octave_freqs[i]/noise_x_samples,
                         v * octave_freqs[i]/noise_y_samples
                      ) * octave_ampls[i]) / 128.0 - 1.0;
            }
            
            // Could apply other density functions...
            
            // Remember the max and min density values within
            // the terrain.
            if( d > max_density )
                max_density = d;
            else if( d < min_density )
                min_density = d;
        }
    }
    
    cout << "Min density: " << min_density << endl;
    cout << "Max density: " << max_density << endl;
    
    // TODO: Fix the fact that this nullifies the density_bias value!
    
    double density_range = max_density - min_density;
    
    // Write the new density image
    out_image.width = width;
    out_image.height = height;
    out_image.data.resize(width*height*3);
    out_image.bytes_per_pixel = 3;
    // Re-map the range of density values to colour values
    // and write to all three colour channels.
    for( int i=0; i!=width*height; ++i ) {
        out_image.data[i*3+0] = (unsigned char)
                                ((density[i]-min_density) /
                                density_range * 255);
        out_image.data[i*3+1] = (unsigned char)
                                ((density[i]-min_density) /
                                density_range * 255);
        out_image.data[i*3+2] = (unsigned char)
                                ((density[i]-min_density) /
                                density_range * 255);
    }
    
    // Success
    return 0;
}

struct Vertex {
    Vertex(double x, double y)
    {
        val[0] = x;
        val[1] = y;
    }
    Vertex(const double v[2])
    {
        val[0] = v[0];
        val[1] = v[1];
    }
    double val[2];
};

typedef list<Vertex> Shape;
typedef list<Vertex>::iterator VertexIterator;

typedef unsigned char uchar;

// Returns outside_colour if the given coordinates are
// outside the image dimensions.
const uchar* getPixel(const Image& image, int x, int y,
                      const uchar outside_colour[])
{
    if( x<0 || x>=image.width || y<0 || y>=image.height )
        return outside_colour;
    else
        return &image.data[(x+y*image.width)*image.bytes_per_pixel];
    
}

// The core of the marching-squares tracing algorithm
bool traceImageFromPoint(const Image& image,
                         vector<bool>& is_visited,
                         int start_x, int start_y,
                         uchar* outside_colour,
                         uchar density_channel,
                         Shape& out_shape,
                         bool act_inclusively=true)
{    
    
    // Stupidity check
    if( density_channel >= image.bytes_per_pixel ) {
        cout << "Error: density_channel is invalid!" << std::endl;
        return false;
    }
    
    /*-----------------------------------------------------*/
    /* Look-up tables */
    /*-----------------------------------------------------*/
    
    // Note: These tables were written by hand, and the
    //       coordinate system\structure used may not be
    //       the most logical. Sorry!
    
    /* TODO: Implement this!
    "Inclusive" behaviour
        If case == {0110}:
            if came from above: (new-old == 0, -1)
                connected_edges == e0, e3
                go left (-1, 0)
            else if came from below: (new-old == 0, +1)
                connected_edges == e2, e1
                go right (+1, 0)
        else if case == {1001}:
            if came from left: (new-old == +1, 0)
                connected_edges == e3, e2
                go down (0, -1)
            else if came from right: (new-old == -1, 0)
                connected_edges == e1, e0
                go up (0, +1)
    "Exclusive" behaviour
        If case == {0110}:
            if came from above: (new-old == 0, -1)
                connected_edges == e0, e1
                go right (+1, 0)
            else if came from below: (new-old == 0, +1)
                connected_edges == e2, e3
                go left (-1, 0)
        else if case == {1001}:
            if came from left: (new-old == +1, 0)
                connected_edges == e3, e0
                go up (0, +1)
            else if came from right: (new-old == -1, 0)
                connected_edges == e1, e2
                go down (0, -1)
        To change behaviour to "exclusive", switch left\right and up\down
        
        m = new-old
        direction = (m[1], m[0])
        
        
        direction[1] *= -1;
    */
    
    /*
       Diagram of indices used in this implementation.
        p == pixel
        e == edge
    
           e0
       ____ ____
       |p0_|_p1|
    e3 |_|_|_|_| e1
       | |_|_| |
       |p2 | p3|
       ---- ----
           e2
    */
    
    // Indices of the two edges connected by a line for each
    // case.
    // Note: -1 is used for the no-line cases and -2 is used
    //       for the ambiguous cases.
    static const int square_case_connected_edges[16/*cases*/][2/*edges*/] = {
        {-1,-1}, {3,0},   {0,1},   {3,1},
        {2,3},   {2,0},   {-2,-2}, {2,1},
        {1,2},   {-2,-2}, {0,2},   {3,2},
        {1,3},   {1,0},   {0,3},   {-1,-1}
    };
    
    // Table of pixels forming each edge
    static const int square_edge_pixels[4/*edges*/][2/*pxls*/] = {
        //{0, 1}, {1, 3}, {3, 2}, {2, 0}
        {1, 0}, {3, 1}, {2, 3}, {0, 2}
    };
    
    // The locations of the 4 pixels in the sample region
    // Note: This actually centres the sample region on p3
    static const int pxl_offsets[4/*pixels*/][2/*components*/] = {
        {-1,-1}, {-0,-1}, {-1,-0}, {-0,-0}
    };
    
    // Move directions for each case.
    // Note: Up is +'ve y, down is -'ve y
    // Note: This set "cuts off" at the ambiguous cases
    static const int square_case_moves[16/*cases*/][2/*components*/] = {
        {+0,+0}, {+0,+1}, {+1,+0}, {+1,+0},
        {-1,+0}, {+0,+1}, {-1,+0}, {+1,+0},
        {+0,-1}, {+0,+1}, {+0,-1}, {+0,-1},
        {-1,+0}, {+0,+1}, {-1,+0}, {+0,+0}
    };
    
    /*-----------------------------------------------------*/
    
    //int w = image.getWidth();
	//int h = image.getHeight();
	int w = image.width;
	int h = image.height;
    
    int x = start_x;
    int y = start_y;
    // TODO: Check carefully what these should be
    int old_x = x;
    int old_y = y;
    
    // Keep tracing till we break
    while (true) {
        DBG_PRINT("coords = " << x << ", " << y << endl);
        
        // Get the 4 surrounding pixel colours as density values
        uchar density[4] = {
            getPixel(image, x+pxl_offsets[0][0], y+pxl_offsets[0][1], outside_colour)[density_channel],
            getPixel(image, x+pxl_offsets[1][0], y+pxl_offsets[1][1], outside_colour)[density_channel],
            getPixel(image, x+pxl_offsets[2][0], y+pxl_offsets[2][1], outside_colour)[density_channel],
            getPixel(image, x+pxl_offsets[3][0], y+pxl_offsets[3][1], outside_colour)[density_channel]
        };
        //DBG_PRINT("pxl_colours[0] = " << (int)pxl_colours[0] << endl);
        //DBG_PRINT("pxl_colours[1] = " << (int)pxl_colours[1] << endl);
        //DBG_PRINT("pxl_colours[2] = " << (int)pxl_colours[2] << endl);
        //DBG_PRINT("pxl_colours[3] = " << (int)pxl_colours[3] << endl);
        
        // Remember that we've already visited this pixel
        is_visited[x+y*w] = true;
        // Generate unique case value by converting to low\high
        // values and then logically concatenating, placing density[0] in
        // the least-sig-bit.
        unsigned char square_case = (density[0]<128)<<0 | (density[1]<128)<<1 |
                                    (density[2]<128)<<2 | (density[3]<128)<<3;
        DBG_PRINT("square_case = " << int(square_case) << endl);
        
        // Deal with the ambiguous cases, where there are
        // multiple 'correct' behaviours.
        if( square_case == 9 || square_case == 6 ) {
            
        }
        
        // Get the indices of the edges that should contain vertices
        int edges[2] = {square_case_connected_edges[square_case][0],
                        square_case_connected_edges[square_case][1]};
        DBG_PRINT("edges[0] = " << edges[0] << endl);
        DBG_PRINT("edges[1] = " << edges[1] << endl);
        if (edges[0]<0 || edges[1]<0) {
            cout << "Error: Invalid/ambiguous square case (" <<
                    (int)square_case << ") reached at " <<
                    x << ", " << y <<
                    ". Aborting shape." << endl;
            return false;
        }
        
        double c_density[4];
        c_density[0] = fabs(density[0] - 128);
        c_density[1] = fabs(density[1] - 128);
        c_density[2] = fabs(density[2] - 128);
        c_density[3] = fabs(density[3] - 128);
        
        // Get the indices of the pixels that form each edge
        int edge_pixels[2/*edges*/][2/*pixels*/] = {
            {square_edge_pixels[edges[0]][0],
             square_edge_pixels[edges[0]][1]},
            {square_edge_pixels[edges[1]][0],
             square_edge_pixels[edges[1]][1]}
        };
        DBG_PRINT("edge_pixels[0][0] = " << edge_pixels[0][0] << endl);
        DBG_PRINT("edge_pixels[0][1] = " << edge_pixels[0][1] << endl);
        DBG_PRINT("edge_pixels[1][0] = " << edge_pixels[1][0] << endl);
        DBG_PRINT("edge_pixels[1][1] = " << edge_pixels[1][1] << endl);
        //cout << "Here 6" << endl;
        // Calculate interpolation values for each edge
        double lerp[2/*edges*/] = {
            (double)c_density[edge_pixels[/*edge*/0][/*pxl*/1]] / (c_density[edge_pixels[0][0]] + c_density[edge_pixels[0][1]]),
            (double)c_density[edge_pixels[/*edge*/1][/*pxl*/1]] / (c_density[edge_pixels[1][0]] + c_density[edge_pixels[1][1]])};
        DBG_PRINT("lerp[0] = " << lerp[0] << endl);
        DBG_PRINT("lerp[1] = " << lerp[1] << endl);
        if( !(lerp[0] >= 0.0 && lerp[0] <= 1.0 &&
              lerp[1] >= 0.0 && lerp[1] <= 1.0) ) {
            cout << "Error: Lerp value is invalid." << endl;
            return false;
        }
        
        /*
        DBG_PRINT("lerped offset[0][0][0] = " << lerp[0]*pxl_offsets[edge_pixels[0][0]][0] << endl);
        DBG_PRINT("lerped offset[0][1][0] = " << (1-lerp[0])*pxl_offsets[edge_pixels[0][1]][0] << endl);
        DBG_PRINT("lerped offset[0][0][1] = " << lerp[0]*pxl_offsets[edge_pixels[0][0]][1] << endl);
        DBG_PRINT("lerped offset[0][1][1] = " << (1-lerp[0])*pxl_offsets[edge_pixels[0][1]][1] << endl);
        DBG_PRINT("lerped offset[1][0][0] = " << lerp[1]*pxl_offsets[edge_pixels[1][0]][0] << endl);
        DBG_PRINT("lerped offset[1][1][0] = " << (1-lerp[1])*pxl_offsets[edge_pixels[1][1]][0] << endl);
        DBG_PRINT("lerped offset[1][0][1] = " << lerp[1]*pxl_offsets[edge_pixels[1][0]][1] << endl);
        DBG_PRINT("lerped offset[1][1][1] = " << (1-lerp[1])*pxl_offsets[edge_pixels[1][1]][1] << endl);
        */
        //lerp[0] = 1 - lerp[0];
        //lerp[1] = 1 - lerp[1];
        // Interpolate vertex positions on each edge
        double vertex_positions[2/*vertices*/][2/*components*/] = {
            {(double)x + lerp[0]*pxl_offsets[edge_pixels[0][0]][0] + (1.0-lerp[0])*pxl_offsets[edge_pixels[0][1]][0],
             (double)y + lerp[0]*pxl_offsets[edge_pixels[0][0]][1] + (1.0-lerp[0])*pxl_offsets[edge_pixels[0][1]][1]},
            {(double)x + lerp[1]*pxl_offsets[edge_pixels[1][0]][0] + (1.0-lerp[1])*pxl_offsets[edge_pixels[1][1]][0],
             (double)y + lerp[1]*pxl_offsets[edge_pixels[1][0]][1] + (1.0-lerp[1])*pxl_offsets[edge_pixels[1][1]][1]}
        };
        DBG_PRINT("vertex_positions[0][0] = " << vertex_positions[0][0] << endl);
        DBG_PRINT("vertex_positions[0][1] = " << vertex_positions[0][1] << endl);
        DBG_PRINT("vertex_positions[1][0] = " << vertex_positions[1][0] << endl);
        DBG_PRINT("vertex_positions[1][1] = " << vertex_positions[1][1] << endl);
        
        // Normalise the vertex coordinates
        vertex_positions[0][0] /= (double)w;
        vertex_positions[0][1] /= (double)h;
        vertex_positions[1][0] /= (double)w;
        vertex_positions[1][1] /= (double)h;
        
        DBG_PRINT("norm'd vertex_positions[0][0] = " << vertex_positions[0][0] << endl);
        DBG_PRINT("norm'd vertex_positions[0][1] = " << vertex_positions[0][1] << endl);
        DBG_PRINT("norm'd vertex_positions[1][0] = " << vertex_positions[1][0] << endl);
        DBG_PRINT("norm'd vertex_positions[1][1] = " << vertex_positions[1][1] << endl);
        
        Vertex new_vert = Vertex(vertex_positions[1]);
        DBG_PRINT("Vertex = (" << new_vert.val[0] << ", " << new_vert.val[1] << ")" << endl);
        
        DBG_PRINT("New vertex created." << endl);
        DBG_PRINT("out_shape size = " << out_shape.size() << endl);
        // Add the second vertex to the shape
        out_shape.push_back(new_vert);
        
        DBG_PRINT("Added vertex to shape." << endl);
        
        // Get direction to move in now
        int direction[2] = {square_case_moves[square_case][0],
                            square_case_moves[square_case][1]};
        // Invert y coordinate!!
        direction[1] = -direction[1];
        DBG_PRINT("direction[0] = " << direction[0] << endl);
        DBG_PRINT("direction[1] = " << direction[1] << endl);
        
        // Remember where we came from
        old_x = x;
        old_y = y;
        // Move along the contour
        x += direction[0];
        y += direction[1];
        
        // Stop once we've returned to where we started
        if (x == start_x && y == start_y)
            break;
        
        //cin.get();
    }
    
    return true;
}

bool traceImage(const Image& image,
                uchar* outside_colour,
                uchar density_channel,
                vector<Shape>& out_shapes)
{
    /*-----------------------------------------------------*/
    /* Look-up table */
    /*-----------------------------------------------------*/
    // The locations of the 4 pixels in the sample region
    // Note: This actually centres the sample region on p3
    static const int pxl_offsets[4/*pixels*/][2/*components*/] = {
        {-1,-1}, {-0,-1}, {-1,-0}, {-0,-0}
    };
    /*-----------------------------------------------------*/
    
    // Lazy
    int w = image.width;
	int h = image.height;
	
	// Array of "have we visited this pixel" for each pixel
	vector<bool> is_visited(w*h, false);
	
	// Iterate over all the pixels + 1 row and 1 column
	for (int y=0;y!=h+1;++y) {
		for (int x=0;x!=w+1;++x) {
		    // Get the current 4-pixel (density) sample
			uchar density[4] = {
                getPixel(image, x+pxl_offsets[0][0], y+pxl_offsets[0][1], outside_colour)[density_channel],
                getPixel(image, x+pxl_offsets[1][0], y+pxl_offsets[1][1], outside_colour)[density_channel],
                getPixel(image, x+pxl_offsets[2][0], y+pxl_offsets[2][1], outside_colour)[density_channel],
                getPixel(image, x+pxl_offsets[3][0], y+pxl_offsets[3][1], outside_colour)[density_channel]
			};
			// Check if the four points are not all low or
			// all high and if the point hasn't been
			// visited before.
			if (!(density[0]<128 &&  density[1]<128 && 
                  density[2]<128 &&  density[3]<128) &&
			    !(density[0]>=128 && density[1]>=128 &&
			      density[2]>=128 && density[3]>=128) &&
			    !is_visited[x+y*w]) {
                
                // We've found a new shape
                out_shapes.push_back(Shape());
                Shape& shape = out_shapes.back();
                cout << "Starting trace of new shape at " << x << ", " << y << endl;
                //cin.get();
                // Trace the shape
                if (!traceImageFromPoint(image, is_visited,
                                         x, y,
                                         outside_colour,
                                         density_channel,
                                         shape))
                    return false;
			}

		}
	}
	
	return true;
	
}

// Removes vertices in a polygon that are reduntant
// TODO: Consider adding check for very close vertices.
bool cullVertices(Shape& shape, double angle_tolerance=5*3.1415926535897932/180.0)
{
    // TODO: Try out some more complex culling algos, like using avg. angle of more verts
    
    VertexIterator it=shape.begin();
    int change_count = 1;
    int times_around = 0;
    while (true) {
        // Find iterators pointing to the previous and next
        // vertices, applied circularly.
        VertexIterator it_prev = it;
        --it_prev;
        if( it_prev == shape.end() )
            --it_prev;
        VertexIterator it_next = it;
        ++it_next;
        if( it_next == shape.end() )
            ++it_prev;
        
        double angle = fabs(atan2(it->val[1]-it_prev->val[1],
                            it->val[0]-it_prev->val[0]) -
                           (atan2(it_next->val[1]-it->val[1],
                           it_next->val[0]-it->val[0])));
        
        if( it == shape.end() ) {
        	if( change_count == 0 )
	            break;
        	change_count = 0;
        	// Back to the start
        	++it;
        	++times_around;
       	}
         
        if( angle < angle_tolerance ) {
            it = shape.erase(it);
            ++change_count;
        }
        else
            ++it;        
    }
    std::cout << "Times around = " << times_around << std::endl;
    return true;
}

int main(int argc, char** argv)
{
	double angle_tolerance = 0;
	Image image;
	
	if( argc <= 1 ) {
		// Use terrain generation
        // -----------------------
        double octave_freqs[10] = {1.01, 1.96, 4.03,
                                  7.98, 16.08, 32.14,
                                  63.91, 128.02, 255.99, 512.03};
        double octave_ampls[10] = {1.0, 1.0/2, 1.0/4,
                                  1.0/8, 1.0/16, 1.0/32,
                                  1.0/64, 1.0/128, 1.0/256, 1.0/512};
        double warp_freqs[1] = {1.0/4};
        double warp_ampls[1] = {0.1/warp_freqs[0]};
        cout << "Beginning terrain generation." << endl;
        genTerrain(image, 64, 64, 8/*time(NULL)*/, -8.0,
                   8, &octave_freqs[0], &octave_ampls[0],
                   1, warp_freqs, warp_ampls);
        cout << "Terrain generation complete." << endl;
        int ret = saveTGAImage(&image, "terrain.tga");
        if( ret != IMAGE_SUCCESS ) {
            cout << "Error: Image saving failed." << endl;
        }
        cout << "Terrain image saved." << endl;
        // -----------------------
        //return 0;
	}
	else if( argc == 3 ) {
	    // Load image from file
	    int ret = loadTGAImage(argv[1], &image);
        if( ret != IMAGE_SUCCESS) {
            cout << "Error: Image loading failed: " << ret << endl;
            return -1;
        }
	    angle_tolerance = atof(argv[2]);
	    angle_tolerance *= 3.1415926535897932 / 180.0;
	}
	else {
	    cout << "Usage: Image2EdgeShapes [image_filename, angle_tolerance]" << endl;
		return -1;
	}
	
	vector<Shape> shapes;
	uchar outside_colour[3] = {255, 0, 0};
	if (!traceImage(image, outside_colour, Image::COLOUR_RED, shapes)) {
        cout << "Error: Image tracing failed." << endl;
        return -1;
	}
	
	cout << endl;
	cout << endl;
	cout << "shape count = " << shapes.size() << endl;
	
	ofstream pre_cull_output("pre_cull.dat");
	ofstream post_cull_output("post_cull.dat");
	
	for (unsigned int i=0;i<shapes.size();++i) {
		for( VertexIterator it=shapes[i].begin(); it!=shapes[i].end(); ++it)
			pre_cull_output << it->val[0] << "\t" << it->val[1] << endl;
        cout << "pre-cull shapes[" << i << "] vertex count = " << shapes[i].size() << endl;
        cullVertices(shapes[i], angle_tolerance);
        for( VertexIterator it=shapes[i].begin(); it!=shapes[i].end(); ++it)
			post_cull_output << it->val[0] << "\t" << it->val[1] << endl;
        cout << "post-cull shapes[" << i << "] vertex count = " << shapes[i].size() << endl;
	}
	
	post_cull_output.close();
	pre_cull_output.close();
	
	return 0;
}







// Old code
#if 0
	Image(const Image& copy)
	{
		my_width = copy.my_width;
		my_width = copy.my_height;
		my_data = new T_ColorType()[my_width*my_height];
		for (int i=0;i!=copy.my_width*copy.my_height;++i) {
			my_data[i] = copy.my_data[i];
		}
	}
	Image operator = (const Image& copy)
	{
		delete [] my_data;
		my_width = copy.my_width;
		my_width = copy.my_height;
		my_data = new T_ColorType()[my_width*my_height];
		for (int i=0;i!=copy.my_width*copy.my_height;++i) {
			my_data[i] = copy.my_data[i];
		}
	}
	~Image()
	{
		delete [] my_data;
	}


/*
bool loadBMP8Image(string filename, Image& image)
{

    int Width,Height;		// Dimensions
    int BPP;				// Bits Per Pixel.
    char * Raster;			// Bits of the Image.
    RGBQUAD * Palette;		// RGB Palette for the image.
    int BytesPerRow;		// Row Width (in bytes).
    BITMAPINFO * pbmi;		// BITMAPINFO structure

    // Member functions (defined later):
    int LoadBMP (char * szFile);
    int GDIPaint (HDC hdc,int x,int y);




    BITMAPFILEHEADER bmfh;
	BITMAPINFOHEADER bmih;

	// Open file.
	ifstream bmpfile (filename , ios::in | ios::binary);
	if (! bmpfile.is_open()) return false;		// Error opening file

	// Load bitmap fileheader & infoheader
	bmpfile.read ((char*)&bmfh,sizeof (BITMAPFILEHEADER));
	bmpfile.read ((char*)&bmih,sizeof (BITMAPINFOHEADER));

	// Check filetype signature
	if (bmfh.bfType!='MB') return false;		// File is not BMP

	// Assign some short variables:
	int BPP=bmih.biBitCount;
	int Width=bmih.biWidth;
	int Height= (bmih.biHeight>0) ? bmih.biHeight : -bmih.biHeight; // absoulte value
	int BytesPerRow = Width * BPP / 8;
	BytesPerRow += (4-BytesPerRow%4) % 4;	// int alignment

	// If BPP aren't 24, load Palette:
	if (BPP==24) pbmi=(BITMAPINFO*)new char [sizeof(BITMAPINFO)];
	else
	{
		pbmi=(BITMAPINFO*) new char[sizeof(BITMAPINFOHEADER)+(1<<BPP)*sizeof(RGBQUAD)];
		Palette=(RGBQUAD*)((char*)pbmi+sizeof(BITMAPINFOHEADER));
		bmpfile.read ((char*)Palette,sizeof (RGBQUAD) * (1<<BPP));
	}
	pbmi->bmiHeader=bmih;

	// Load Raster
	bmpfile.seekg (bmfh.bfOffBits,ios::beg);

	Raster= new char[BytesPerRow*Height];

	// (if height is positive the bmp is bottom-up, read it reversed)
	if (bmih.biHeight>0)
		for (int n=Height-1;n>=0;n--)
			bmpfile.read (Raster+BytesPerRow*n,BytesPerRow);
	else
		bmpfile.read (Raster,BytesPerRow*Height);

	// so, we always have a up-bottom raster (that is negative height for windows):
	pbmi->bmiHeader.biHeight=-Height;

	bmpfile.close();
    
    
    
    image.setData(width, height, data);
    return true;
}
*/

class ImageClass
{
	vector<unsigned char> my_data;
	int my_width;
	int my_height;
	int my_bytes_per_pixel
public:
    
    enum RetVal { RET_SUCCESS = 0,
                  RET_FAILED_TO_OPEN_FILE,
                  RET_INVALID_TYPE,
                  RET_INVALID_BIT_DEPTH
    };
    
	Image(string filename)
	{
		//my_data.resize(width*height, 0);
		if (filename == "sample") {
		    my_width = 64;//1000;
		    my_height = 64;//1000;
		    my_data.resize(my_width*my_height, 255);
		    for (int y=0;y<my_height;++y) {
		        for (int x=0;x<my_width;++x) {
		            int cx = x - my_width/2;
		            int cy = y - my_height/2;
		            if (cx*cx+cy*cy <= (my_width/4)*(my_width/4))
                        //my_data[x+y*my_width] = (cx*cx+cy*cy) / ((my_width/4)*(my_width/4)) * 255;
                        my_data[x+y*my_width] = 0;
		            
		            //if (y>x)
                    //    my_data[x+y*my_width] = 0;
		            /*
		            if (cx*cx+cy*cy <= (my_width/4)*(my_width/4))
                        my_data[x+y*my_width] = 0;
                    else if (cx*cx+cy*cy <= (my_width/4+1)*(my_width/4+1))
                        my_data[x+y*my_width] = sqrt(cx*cx+cy*cy) / (my_width/4+1)*(my_width/4+1)*255;
                    */
                    DBG_PRINT((my_data[x+y*my_width]==255));
		        }
            DBG_PRINT(endl);
		    }
		}
		
		//std::cin.get();
	}
	
	bool setData(int width, int height, int bytes_per_pixel, const unsigned int data[])
    {
        my_data.resize(width*height*bytes_per_pixel, 0);
        for (int i=0;i!=width*height*bytes_per_pixel;++i) {
            my_data[i] = data[i];
        }
    }
	
	int getWidth() const { return my_width; }
	int getHeight() const { return my_height; }
	
	T_ColorType getPixel(int x, int y, T_ColorType outside_val=255) const
	{
		if (x<0 || x>=my_width || y<0 || y>=my_height)
			return outside_val;
		else
			return my_data[x + y*my_width];
	}
};

/*
typedef unsigned char ColorType;
Image<ColorType> image = loadImage<ColorType>(filename);

typedef b2Vec2 Vertex;
traceImage(image, 0,
*/

            //image.getPixel(x+pxl_offsets[0][0], y+pxl_offsets[0][1], outside_colour),
            //image.getPixel(x+pxl_offsets[1][0], y+pxl_offsets[1][1], outside_colour),
            //image.getPixel(x+pxl_offsets[2][0], y+pxl_offsets[2][1], outside_colour),
            //image.getPixel(x+pxl_offsets[3][0], y+pxl_offsets[3][1], outside_colour)

#endif
